#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "tf2/utils.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include <chrono>
using namespace std::chrono_literals;

class GetPose:public rclcpp::Node{
public:
    GetPose(const std::string &node_name):Node(node_name)
    {
        this->set_parameter(rclcpp::Parameter("use_sim_time", true));
        
        bool use_sim_time;
        this->get_parameter("use_sim_time", use_sim_time);
        RCLCPP_INFO(this->get_logger(), "use_sim_time: %s", use_sim_time ? "true" : "false");


        buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
        // 延迟构造 listener，确保 node 完整构造后才添加到 executor
        listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_);
        timer_ = this->create_wall_timer(1s, std::bind(&GetPose::getTransform, this));
      
    }
private:
    rclcpp::TimerBase::SharedPtr timer_;
    std::shared_ptr<tf2_ros::Buffer> buffer_;
    std::shared_ptr<tf2_ros::TransformListener> listener_;

    void getTransform()
    {
       try {
            auto transform = buffer_->lookupTransform(
                "map", "base_footprint", this->get_clock()->now(), rclcpp::Duration::from_seconds(1.0));
            auto translation = transform.transform.translation;
            auto rotation = transform.transform.rotation;

            double roll, pitch, yaw;
            tf2::getEulerYPR(tf2::Quaternion(rotation.x, rotation.y, rotation.z, rotation.w), yaw, pitch, roll);
            RCLCPP_INFO(this->get_logger(),"接收端信息接受@#@@@");
            RCLCPP_INFO(this->get_logger(), "Translation: x=%.2f, y=%.2f, z=%.2f",
                        translation.x, translation.y, translation.z);
            RCLCPP_INFO(this->get_logger(), "Rotation: yaw=%.2f, pitch=%.2f, roll=%.2f",
                        yaw, pitch, roll);
        } catch (const std::exception &e) {
            RCLCPP_WARN(this->get_logger(), "Transform error: %s", e.what());
        }
    }
};
 
int main(int argc,char ** argv)
{
   // 2.初始化ROS2客户端
   rclcpp::init(argc,argv);
   std::cout<<"this is a pose "<<std::endl;
   //4.调用spin函数，传入自定义对象指针
   auto node = std::make_shared<GetPose>("get_pose");
   rclcpp::spin(node);
   //5.释放资源
   rclcpp::shutdown();
   return 0;
}